#include <wiringPi.h>
#include <stdio.h>

int main()
{
	const int trigger_pin=4;
	const int echo_pin=5;
	const int led_pins[]={25,29,28,27};
	const int n_led_pins=sizeof(led_pins)/sizeof(led_pins[0]);

	wiringPiSetup();

	pinMode(echo_pin, INPUT);
	pinMode(trigger_pin, OUTPUT);
	digitalWrite(trigger_pin, LOW);

	for(int i=0;i<n_led_pins;++i){
		pinMode(led_pins[i], OUTPUT);
		digitalWrite(led_pins[i], LOW);
	}

	unsigned int echo_start=0, echo_end=0;
	int n_leds_turn_on=0;

	while(1){
		digitalWrite(trigger_pin, HIGH);
		delayMicroseconds(10);
		digitalWrite(trigger_pin, LOW);

		while(digitalRead(echo_pin)==LOW)
			echo_start=micros();

		while(digitalRead(echo_pin)==HIGH)
			echo_end=micros();

		double distance=(331.5+0.61*25)*((echo_end-echo_start)/2.0/1000000)*100;

		printf("Distance: %.2f cm\n", distance);

		if(distance<=10)
			n_leds_turn_on=1;
		else if(distance<=20)
			n_leds_turn_on=2;
		else if(distance<=30)
			n_leds_turn_on=3;
		else
			n_leds_turn_on=4;

		for(int i=0;i<n_led_pins;++i)
			digitalWrite(led_pins[i], (i<n_leds_turn_on?HIGH:LOW));

		delay(500);
	}

	return 0;
}